#include <UnitTest++.h>
#include <vector>
#include <Math/Vector3.hpp>
#include <SfM/Homography.hpp>
#include <SfM/Affine.hpp>

using namespace zzz;
#if defined(ZZZ_LIB_LAPACK)
SUITE(HomographyTest)
{
  TEST(AffineTest)
  {
    vector<Vector2d> a,b;
    a.push_back(Vector2d(0,0)); b.push_back(Vector2d(1,1));
    a.push_back(Vector2d(10,1)); b.push_back(Vector2d(5,2));
    a.push_back(Vector2d(3,-8)); b.push_back(Vector2d(-7,10));
    Affine h;
    h.Create(a,b);
    CHECK_CLOSE(h.ToB(a[0]).Len(),b[0].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[1]).Len(),b[1].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[2]).Len(),b[2].Len(),EPSILON);
  }
  TEST(Full)
  {
    vector<Vector3d> a,b;
    a.push_back(Vector3d(0,0,1));  b.push_back(Vector3d(0,0,1));
    a.push_back(Vector3d(1,2,1));  b.push_back(Vector3d(1,33,1));
    a.push_back(Vector3d(5,3,1));  b.push_back(Vector3d(0,3,1));
    a.push_back(Vector3d(65,10,1)); b.push_back(Vector3d(12,42,1));
    Homography h;
    h.Create(a,b);
    CHECK_CLOSE(h.ToB(a[0]).Len(),b[0].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[1]).Len(),b[1].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[2]).Len(),b[2].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[3]).Len(),b[3].Len(),EPSILON);
  }
  TEST(Less)
  {
    vector<Vector3d> a,b;
    a.push_back(Vector3d(0,0,1));  b.push_back(Vector3d(0,0,1));
    a.push_back(Vector3d(1,2,1));  b.push_back(Vector3d(1,33,1));
    a.push_back(Vector3d(5,3,1));  b.push_back(Vector3d(0,3,1));
    Homography h;
    h.Create(a,b);
    CHECK_CLOSE(h.ToB(a[0]).Len(),b[0].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[1]).Len(),b[1].Len(),EPSILON);
    CHECK_CLOSE(h.ToB(a[2]).Len(),b[2].Len(),EPSILON);
  }
  TEST(Over)
  {
    vector<Vector3d> a,b;
    a.push_back(Vector3d(0,0,1));    b.push_back(Vector3d(0,0,1));
    a.push_back(Vector3d(11,22,1));    b.push_back(Vector3d(13,53,1));
    a.push_back(Vector3d(1,2,1));    b.push_back(Vector3d(1,33,1));
    a.push_back(Vector3d(5,3,1));    b.push_back(Vector3d(0,3,1));
    a.push_back(Vector3d(231,22,1));  b.push_back(Vector3d(41,33,1));
    a.push_back(Vector3d(52,13,1));    b.push_back(Vector3d(10,323,1));
    Homography h;
    h.Create(a,b);
  }
}
#endif